
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       fms_gcs.c
  * @author     baiyang
  * @date       2021-11-10
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"

#include <stdint.h>
#include <stdbool.h>

#include <rthw.h>
#include <rtthread.h>
#include <rtdevice.h>

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>
#include <common/time/gp_time.h>
#include <mavproxy/mavproxy.h>
#include <mavproxy/mavproxy_monitor.h>
/*-----------------------------------macro------------------------------------*/
// for mavlink SET_POSITION_TARGET messages
#define MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE (POSITION_TARGET_TYPEMASK_X_IGNORE | POSITION_TARGET_TYPEMASK_Y_IGNORE | POSITION_TARGET_TYPEMASK_Z_IGNORE)
#define MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE (POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | POSITION_TARGET_TYPEMASK_VZ_IGNORE)
#define MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE (POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE)

#define MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE POSITION_TARGET_TYPEMASK_YAW_IGNORE
#define MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE
#define MAVLINK_SET_POS_TYPE_MASK_FORCE_SET POSITION_TARGET_TYPEMASK_FORCE_SET
/*----------------------------------typedef-----------------------------------*/
typedef struct {
    uint8_t sysid;          ///< ID of message sender system/aircraft
    uint8_t compid;         ///< ID of the message sender component

    struct rt_ringbuffer msg_rb;       // id(4 byte) + sysid(1 byte) + compid(1 byte) + msg
    uint8_t msg_rb_pool[1024];

    mavlink_message_t mavlink_msg;

    struct rt_mutex _mutex;
} fms_gcs_msg;

/*---------------------------------prototype----------------------------------*/
static void fms_rc_manual_override(RC_HandleTypeDef *c, int16_t value_in, const uint16_t offset, const float scaler, const uint32_t tnow, const bool reversed);
static void fms_handle_rc_channels_override(const mavlink_rc_channels_override_t *packet);
static void fms_handle_set_mode(const mavlink_set_mode_t* set_mode, mavlink_message_t *msg);
static void fms_push_msg(uint32_t msg_id, uint8_t sysid, uint8_t compid, uint8_t *msg_buf, uint32_t msg_len);
static uint32_t fms_get_msg_id();
static void fms_get_msg_date(uint8_t *msg_buf, uint32_t msg_len);
static void fms_send_command_ack_sysid(uint16_t command, uint8_t result, mavlink_message_t* msg);
static void fms_proc_command_long(mavlink_command_long_t* command, mavlink_message_t* msg);
static void fms_proc_command_int(mavlink_command_int_t* command, mavlink_message_t* msg);
static MAV_RESULT fms_handle_command_component_arm_disarm(const mavlink_command_long_t *packet);
static void fms_send_command_ack(uint16_t command, uint8_t result, mavlink_message_t* msg);
static MAV_RESULT fms_handle_command_do_set_mode(const mavlink_command_long_t *packet);
static MAV_RESULT fms_set_mode_common(const MAV_MODE _base_mode, const uint32_t _custom_mode);
static void fms_gcs_cb(void *parameter);
static bool fms_should_handle_command_long(uint16_t command);
static bool fms_should_handle_command_int(uint16_t command);
static bool mavlink_coordinate_frame_to_location_alt_frame(const MAV_FRAME coordinate_frame, AltFrame *frame);
static MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t *packet);
static bool location_from_command_t(const mavlink_command_int_t *in, Location *out);
static bool command_long_stores_location(const MAV_CMD command);
static MAV_RESULT handle_command_pause_continue(const mavlink_command_int_t *packet);
static bool handle_mission_item(mavlink_message_t *msg);
static void send_mission_ack(mavlink_message_t *msg, MAV_MISSION_RESULT result);
static bool handle_guided_request(Mission_Command *cmd);
static void do_yaw(const Mission_Command* cmd);
static MAV_RESULT handle_command_get_home_position(const mavlink_command_long_t *packet, mavlink_message_t* msg);
static void send_home_position(mavlink_message_t* msg);
static void send_gps_global_origin(mavlink_message_t* msg);
/*----------------------------------variable----------------------------------*/
static fms_gcs_msg gcs_msg;

static const float magic_force_arm_value = 2989.0f;
static const float magic_force_disarm_value = 21196.0f;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void fms_gcs_init()
{
    rt_ringbuffer_init(&gcs_msg.msg_rb, gcs_msg.msg_rb_pool, sizeof(gcs_msg.msg_rb_pool));

    rt_mutex_init(&gcs_msg._mutex, "fms_gcs", RT_IPC_FLAG_FIFO);

    itc_subscribe(ITC_ID(mavlink_msg), fms_gcs_cb);
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void fms_gcs_update()
{
    uint32_t msg_id;
    uint16_t len = rt_ringbuffer_space_len(&gcs_msg.msg_rb);
    if (rt_ringbuffer_data_len(&gcs_msg.msg_rb) < sizeof(uint32_t)) {
        // 没有要处理的数据，直接返回
        return;
    }

    msg_id = fms_get_msg_id();

    switch (msg_id) {
        case MAVLINK_MSG_ID_COMMAND_LONG: {
            mavlink_command_long_t command;
            fms_get_msg_date((uint8_t *)&command, sizeof(mavlink_command_long_t));

            mavlink_msg_command_long_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &command);
            fms_proc_command_long(&command, &gcs_msg.mavlink_msg);
        } break;

        case MAVLINK_MSG_ID_COMMAND_INT:
        {
            mavlink_command_int_t command;
            fms_get_msg_date((uint8_t *)&command, sizeof(mavlink_command_int_t));

            mavlink_msg_command_int_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &command);

            
            break;
        }

        case MAVLINK_MSG_ID_MANUAL_CONTROL:
        {
            mavlink_manual_control_t packet;
            fms_get_msg_date((uint8_t *)&packet, sizeof(mavlink_manual_control_t));

            mavlink_system_t sysid_this_mav = mavproxy_get_system();

            if (packet.target != sysid_this_mav.sysid) {
                break; // only accept control aimed at us
            }

            if (packet.z < 0) { // Copter doesn't do negative thrust
                break;
            }

            uint32_t tnow = time_millis();

            fms_rc_manual_override(fms.channel_roll, packet.y, 1000, 2000, tnow, false);
            fms_rc_manual_override(fms.channel_pitch, packet.x, 1000, 2000, tnow, true);
            fms_rc_manual_override(fms.channel_throttle, packet.z, 0, 1000, tnow, false);
            fms_rc_manual_override(fms.channel_yaw, packet.r, 1000, 2000, tnow, false);
            break;
        }
    
        case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
        {
            mavlink_rc_channels_override_t packet;
            fms_get_msg_date((uint8_t *)&packet, sizeof(mavlink_rc_channels_override_t));

            fms_handle_rc_channels_override(&packet);
            break;
        }

        case MAVLINK_MSG_ID_SET_MODE:
        {
            mavlink_set_mode_t packet;

            fms_get_msg_date((uint8_t *)&packet, sizeof(mavlink_set_mode_t));
            mavlink_msg_set_mode_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &packet);

            fms_handle_set_mode(&packet, &gcs_msg.mavlink_msg);
            break;
        }

        case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:   // MAV ID: 82
        {
            mavlink_set_attitude_target_t packet;
            fms_get_msg_date((uint8_t *)&packet, sizeof(mavlink_set_attitude_target_t));

            // exit if vehicle is not in Guided mode or Auto-Guided mode
            if (!mode_in_guided_mode(fms.flightmode)) {
                break;
            }

            const bool roll_rate_ignore   = packet.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE;
            const bool pitch_rate_ignore  = packet.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE;
            const bool yaw_rate_ignore    = packet.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE;
            const bool throttle_ignore    = packet.type_mask & ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE;
            const bool attitude_ignore    = packet.type_mask & ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE;

            // ensure thrust field is not ignored
            if (throttle_ignore) {
                break;
            }

            Quat_t attitude_quat;
            if (attitude_ignore) {
                quat_zero(&attitude_quat);
            } else {
                attitude_quat.q0 = packet.q[0];
                attitude_quat.q1 = packet.q[1];
                attitude_quat.q2 = packet.q[2];
                attitude_quat.q3 = packet.q[3];

                // Do not accept the attitude_quaternion
                // if its magnitude is not close to unit length +/- 1E-3
                // this limit is somewhat greater than sqrt(FLT_EPSL)
                if (!quat_is_unit_length(&attitude_quat)) {
                    // The attitude quaternion is ill-defined
                    break;
                }
            }

            // check if the message's thrust field should be interpreted as a climb rate or as thrust
            const bool use_thrust = mguided_set_attitude_target_provides_thrust();

            float climb_rate_or_thrust;
            if (use_thrust) {
                // interpret thrust as thrust
                climb_rate_or_thrust = math_constrain_float(packet.thrust, -1.0f, 1.0f);
            } else {
                // convert thrust to climb rate
                packet.thrust = math_constrain_float(packet.thrust, 0.0f, 1.0f);
                if (math_flt_equal(packet.thrust, 0.5f)) {
                    climb_rate_or_thrust = 0.0f;
                } else if (packet.thrust > 0.5f) {
                    // climb at up to WPNAV_SPEED_UP
                    climb_rate_or_thrust = (packet.thrust - 0.5f) * 2.0f * wpnav_get_default_speed_up(fms.wp_nav);
                } else {
                    // descend at up to WPNAV_SPEED_DN
                    climb_rate_or_thrust = (0.5f - packet.thrust) * 2.0f * -wpnav_get_default_speed_down(fms.wp_nav);
                }
            }

            Vector3f_t ang_vel = VECTOR3F_DEFAULT_VALUE;
            if (!roll_rate_ignore) {
                ang_vel.x = packet.body_roll_rate;
            }
            if (!pitch_rate_ignore) {
                ang_vel.y = packet.body_pitch_rate;
            }
            if (!yaw_rate_ignore) {
                ang_vel.z = packet.body_yaw_rate;
            }

            mguided_set_angle((mode_base_t)&fms.mode_guided, &attitude_quat, &ang_vel,
                    climb_rate_or_thrust, use_thrust);

            break;
        }

        case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:     // MAV ID: 84
        {
            // decode packet
            mavlink_set_position_target_local_ned_t packet;
            fms_get_msg_date((uint8_t *)&packet, sizeof(mavlink_set_position_target_local_ned_t));

            // exit if vehicle is not in Guided mode or Auto-Guided mode
            if (!mode_in_guided_mode(fms.flightmode)) {
                break;
            }

            // check for supported coordinate frames
            if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED &&
                packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED &&
                packet.coordinate_frame != MAV_FRAME_BODY_NED &&
                packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) {
                // input is not valid so stop
                mode_init((mode_base_t)&fms.mode_guided, true);
                break;
            }

            bool pos_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
            bool vel_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
            bool acc_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
            bool yaw_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
            bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
            bool force_set       = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE_SET;

            // Force inputs are not supported
            // Do not accept command if force_set is true and acc_ignore is false
            if (force_set && !acc_ignore) {
                break;
            }

            // prepare position
            Vector3f_t pos_vector = VECTOR3F_DEFAULT_VALUE;
            if (!pos_ignore) {
                // convert to cm
                pos_vector.x = packet.x * 100.0f;
                pos_vector.y = packet.y * 100.0f;
                pos_vector.z = -packet.z * 100.0f;

                // rotate to body-frame if necessary
                if (packet.coordinate_frame == MAV_FRAME_BODY_NED ||
                    packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
                    fms_rotate_body_frame_to_NE(&pos_vector.x, &pos_vector.y);
                }
                // add body offset if necessary
                if (packet.coordinate_frame == MAV_FRAME_LOCAL_OFFSET_NED ||
                    packet.coordinate_frame == MAV_FRAME_BODY_NED ||
                    packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
                    pos_vector.x += fms.ahrs->relpos_cm.x;
                    pos_vector.y += fms.ahrs->relpos_cm.y;
                    pos_vector.z += fms.ahrs->relpos_cm.z;
                }
            }

            // prepare velocity
            Vector3f_t vel_vector = VECTOR3F_DEFAULT_VALUE;
            if (!vel_ignore) {
                // convert to cm
                vel_vector.x = packet.vx * 100.0f;
                vel_vector.y = packet.vy * 100.0f;
                vel_vector.z = -packet.vz * 100.0f;

                // rotate to body-frame if necessary
                if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
                    fms_rotate_body_frame_to_NE(&vel_vector.x, &vel_vector.y);
                }
            }

            // prepare acceleration
            Vector3f_t accel_vector = VECTOR3F_DEFAULT_VALUE;
            if (!acc_ignore) {
                // convert to cm
                accel_vector.x = packet.afx * 100.0f;
                accel_vector.y = packet.afy * 100.0f;
                accel_vector.z = -packet.afz * 100.0f;

                // rotate to body-frame if necessary
                if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
                    fms_rotate_body_frame_to_NE(&accel_vector.x, &accel_vector.y);
                }
            }

            // prepare yaw
            float yaw_cd = 0.0f;
            bool yaw_relative = false;
            float yaw_rate_cds = 0.0f;
            if (!yaw_ignore) {
                yaw_cd = ToDeg(packet.yaw) * 100.0f;
                yaw_relative = packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED;
            }
            if (!yaw_rate_ignore) {
                yaw_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
            }

            // send request
            if (!pos_ignore && !vel_ignore) {
                mguided_set_destination_posvelaccel((mode_base_t)&fms.mode_guided, &pos_vector, &vel_vector, &accel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
            } else if (pos_ignore && !vel_ignore) {
                mguided_set_velaccel((mode_base_t)&fms.mode_guided, &vel_vector, &accel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative, true);
            } else if (pos_ignore && vel_ignore && !acc_ignore) {
                mguided_set_accel((mode_base_t)&fms.mode_guided, &accel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative, true);
            } else if (!pos_ignore && vel_ignore && acc_ignore) {
                mguided_set_destination((mode_base_t)&fms.mode_guided, &pos_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative, false);
            } else {
                // input is not valid so stop
                mode_init((mode_base_t)&fms.mode_guided, true);
            }
            break;
        }

        case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:    // MAV ID: 86
        {
            // decode packet
            mavlink_set_position_target_global_int_t packet;
            fms_get_msg_date((uint8_t *)&packet, sizeof(mavlink_set_position_target_global_int_t));

            // exit if vehicle is not in Guided mode or Auto-Guided mode
            if (!mode_in_guided_mode(fms.flightmode)) {
                break;
            }

            // todo: do we need to check for supported coordinate frames

            bool pos_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
            bool vel_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
            bool acc_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
            bool yaw_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
            bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
            bool force_set       = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE_SET;

            // Force inputs are not supported
            // Do not accept command if force_set is true and acc_ignore is false
            if (force_set && !acc_ignore) {
                break;
            }

            // extract location from message
            Location loc;
            if (!pos_ignore) {
                // sanity check location
                if (!location_check_lat_lng2(packet.lat_int, packet.lon_int)) {
                    // input is not valid so stop
                    mode_init((mode_base_t)&fms.mode_guided, true);
                    break;
                }
                AltFrame frame;
                if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)packet.coordinate_frame, &frame)) {
                    // unknown coordinate frame
                    // input is not valid so stop
                    mode_init((mode_base_t)&fms.mode_guided, true);
                    break;
                }
                location_from_lat_lon_alt(&loc, packet.lat_int, packet.lon_int, (int32_t)(packet.alt*100), frame);
            }

            // prepare velocity
            Vector3f_t vel_vector = VECTOR3F_DEFAULT_VALUE;
            if (!vel_ignore) {
                // convert to cm
                vel_vector.x = packet.vx * 100.0f;
                vel_vector.y = packet.vy * 100.0f;
                vel_vector.z = -packet.vz * 100.0f;
            }

            // prepare acceleration
            Vector3f_t accel_vector = VECTOR3F_DEFAULT_VALUE;
            if (!acc_ignore) {
                // convert to cm
                accel_vector.x = packet.afx * 100.0f;
                accel_vector.y = packet.afy * 100.0f;
                accel_vector.z = -packet.afz * 100.0f;
            }

            // prepare yaw
            float yaw_cd = 0.0f;
            float yaw_rate_cds = 0.0f;
            bool yaw_relative = false;
            if (!yaw_ignore) {
                yaw_cd = ToDeg(packet.yaw) * 100.0f;
                yaw_relative = packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED;
            }
            if (!yaw_rate_ignore) {
                yaw_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
            }
    
            // send targets to the appropriate guided mode controller
            if (!pos_ignore && !vel_ignore && !acc_ignore) {
                // convert Location to vector from ekf origin for posvel controller
                if (location_get_alt_frame(&loc) == ALT_FRAME_ABOVE_TERRAIN) {
                    // posvel controller does not support alt-above-terrain
                    // input is not valid so stop
                    mode_init((mode_base_t)&fms.mode_guided, true);
                    break;
                }
                Vector3f_t pos_neu_cm;
                if (!location_get_vector_from_origin_neu(&loc, &pos_neu_cm)) {
                    // input is not valid so stop
                    mode_init((mode_base_t)&fms.mode_guided, true);
                    break;
                }
                mguided_set_destination_posvelaccel((mode_base_t)&fms.mode_guided, &pos_neu_cm, &vel_vector, &accel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
            } else if (!pos_ignore && !vel_ignore) {
                // convert Location to vector from ekf origin for posvel controller
                if (location_get_alt_frame(&loc) == ALT_FRAME_ABOVE_TERRAIN) {
                    // posvel controller does not support alt-above-terrain
                    // input is not valid so stop
                    mode_init((mode_base_t)&fms.mode_guided, true);
                    break;
                }
                Vector3f_t pos_neu_cm;
                if (!location_get_vector_from_origin_neu(&loc, &pos_neu_cm)) {
                    // input is not valid so stop
                    mode_init((mode_base_t)&fms.mode_guided, true);
                    break;
                }
                mguided_set_destination_posvel((mode_base_t)&fms.mode_guided, &pos_neu_cm, &vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, false);
            } else if (pos_ignore && !vel_ignore) {
                mguided_set_velaccel((mode_base_t)&fms.mode_guided, &vel_vector, &accel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, false, true);
            } else if (pos_ignore && vel_ignore && !acc_ignore) {
                mguided_set_accel((mode_base_t)&fms.mode_guided, &accel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, false, true);
            } else if (!pos_ignore && vel_ignore && acc_ignore) {
                mguided_set_destination2((mode_base_t)&fms.mode_guided, &loc, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, false);
            } else {
                // input is not valid so stop
                mode_init((mode_base_t)&fms.mode_guided, true);
            }
            break;
        }

        // GCS has sent us a mission item, store to EEPROM
        case MAVLINK_MSG_ID_MISSION_ITEM:           // MAV ID: 39
        case MAVLINK_MSG_ID_MISSION_ITEM_INT:
        {
            if (msg_id == MAVLINK_MSG_ID_MISSION_ITEM) {
                mavlink_mission_item_t mission;
                fms_get_msg_date((uint8_t *)&mission, sizeof(mavlink_mission_item_t));
                mavlink_msg_mission_item_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &mission);
            } else {
                mavlink_mission_item_int_t mission_int;
                fms_get_msg_date((uint8_t *)&mission_int, sizeof(mavlink_mission_item_int_t));
                mavlink_msg_mission_item_int_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &mission_int);
            }

            handle_mission_item(&gcs_msg.mavlink_msg);
        } break;

        default: {
            // console_printf("unknown mavlink msg:%d\n", msg->msgid);
        } break;
    }
}

static void fms_rc_manual_override(RC_HandleTypeDef *c, int16_t value_in, const uint16_t offset, const float scaler, const uint32_t tnow, const bool reversed)
{
    if (c == NULL) {
        return;
    }
    int16_t override_value = 0;
    if (value_in != INT16_MAX) {
        const int16_t radio_min = c->_radio_min;
        const int16_t radio_max = c->_radio_max;
        if (reversed) {
            value_in *= -1;
        }
        override_value = radio_min + (radio_max - radio_min) * (value_in + offset) / scaler;
    }
    RC_set_override(c, override_value, tnow);
}

// allow override of RC channel values for complete GCS
// control of switch position and RC PWM values.
static void fms_handle_rc_channels_override(const mavlink_rc_channels_override_t *packet)
{
    mavlink_system_t sysid_this_mav = mavproxy_get_system();

    if(packet->target_system != sysid_this_mav.sysid) {
        return; // Only accept control from our gcs
    }

    const uint32_t tnow = time_millis();

    const uint16_t override_data[] = {
        packet->chan1_raw,
        packet->chan2_raw,
        packet->chan3_raw,
        packet->chan4_raw,
        packet->chan5_raw,
        packet->chan6_raw,
        packet->chan7_raw,
        packet->chan8_raw,
        packet->chan9_raw,
        packet->chan10_raw,
        packet->chan11_raw,
        packet->chan12_raw,
        packet->chan13_raw,
        packet->chan14_raw,
        packet->chan15_raw,
        packet->chan16_raw
    };

    for (uint8_t i=0; i<8; i++) {
        // Per MAVLink spec a value of UINT16_MAX means to ignore this field.
        if (override_data[i] != UINT16_MAX) {
            RC_set_override(RCs_get_channel(i), override_data[i], tnow);
        }
    }
    for (uint8_t i=8; i<ARRAY_SIZE(override_data); i++) {
        // Per MAVLink spec a value of zero or UINT16_MAX means to
        // ignore this field.
        if (override_data[i] != 0 && override_data[i] != UINT16_MAX) {
            // per the mavlink spec, a value of UINT16_MAX-1 means
            // return the field to RC radio values:
            const uint16_t value = override_data[i] == (UINT16_MAX-1) ? 0 : override_data[i];
            RC_set_override(RCs_get_channel(i), value, tnow);
        }
    }

    //gcs().sysid_myggcs_seen(tnow);

}

/**
  * @brief       
  * @param[in]   set_mode  
  * @param[in]   msg  
  * @param[out]  
  * @retval      
  * @note        
  */
static void fms_handle_set_mode(const mavlink_set_mode_t* set_mode, mavlink_message_t *msg)
{
    const MAV_MODE _base_mode = (MAV_MODE)set_mode->base_mode;
    const uint32_t _custom_mode = set_mode->custom_mode;

    const MAV_RESULT result = fms_set_mode_common(_base_mode, _custom_mode);

    // send ACK or NAK.  Note that this is extraodinarily improper -
    // we are sending a command-ack for a message which is not a
    // command.  The command we are acking (ID=11) doesn't actually
    // exist, but if it did we'd probably be acking something
    // completely unrelated to setting modes.
    fms_send_command_ack_sysid(MAVLINK_MSG_ID_SET_MODE, result, msg);
}

static void fms_send_command_ack_sysid(uint16_t command, uint8_t result,
    mavlink_message_t* msg)
{
    mavlink_command_ack_t command_ack = {command, result, 0, 0, 0, 0};

    mavlink_msg_command_ack_encode(msg->sysid, msg->compid,
        msg, &command_ack);

    mavproxy_send_immediate_msg(msg, 1);
}

static void fms_proc_command_long(mavlink_command_long_t* command, mavlink_message_t* msg)
{
    MAV_RESULT result = MAV_RESULT_FAILED;

    switch (command->command) {
    case MAV_CMD_COMPONENT_ARM_DISARM:
        result = fms_handle_command_component_arm_disarm(command);
        break;

    case MAV_CMD_DO_SET_MODE:
        result = fms_handle_command_do_set_mode(command);
        break;

    case MAV_CMD_NAV_TAKEOFF: {
        // param3 : horizontal navigation by pilot acceptable
        // param4 : yaw angle   (not supported)
        // param5 : latitude    (not supported)
        // param6 : longitude   (not supported)
        // param7 : altitude [metres]

        float takeoff_alt = command->param7 * 100;      // Convert m to cm

        if (!mode_do_user_takeoff(fms.flightmode, takeoff_alt, math_flt_zero(command->param3))) {
            result = MAV_RESULT_FAILED;
        } else {
            result = MAV_RESULT_ACCEPTED;
        }
    }

    case MAV_CMD_GET_HOME_POSITION:
        result = handle_command_get_home_position(command, msg);
        break;

    default:
        break;
    }

    fms_send_command_ack(command->command, result, msg);
}

static void fms_proc_command_int(mavlink_command_int_t* command, mavlink_message_t* msg)
{
    MAV_RESULT result = MAV_RESULT_FAILED;

    switch (command->command) {
    case MAV_CMD_DO_REPOSITION:
        result = handle_command_int_do_reposition(command);
        break;

    case MAV_CMD_DO_PAUSE_CONTINUE:
        result = handle_command_pause_continue(command);
        break;

    default:
        break;
    }

    fms_send_command_ack(command->command, result, msg);
}

static void fms_send_command_ack(uint16_t command, uint8_t result, mavlink_message_t* msg)
{
    mavlink_command_ack_t command_ack = {command, result, 0, 0, 0, 0};
    mavlink_system_t sysid_this_mav = mavproxy_get_system();

    mavlink_msg_command_ack_encode(sysid_this_mav.sysid, sysid_this_mav.compid,
        msg, &command_ack);

    mavproxy_send_immediate_msg(msg, 1);
}

static MAV_RESULT fms_handle_command_component_arm_disarm(const mavlink_command_long_t *packet)
{
    if (math_flt_equal(packet->param1,1.0f)) {
        if (arming_is_armed(&fms.arming)) {
            return MAV_RESULT_ACCEPTED;
        }
        // run pre_arm_checks and arm_checks and display failures
        const bool do_arming_checks = !math_flt_equal(packet->param2,magic_force_arm_value);
        if (arming_arm(&fms.arming, ARMING_CHECK_MAVLINK, do_arming_checks)) {
            return MAV_RESULT_ACCEPTED;
        }
        return MAV_RESULT_FAILED;
    }
    if (math_flt_zero(packet->param1))  {
        if (!arming_is_armed(&fms.arming)) {
            return MAV_RESULT_ACCEPTED;
        }
        const bool forced = math_flt_equal(packet->param2, magic_force_disarm_value);
        // note disarm()'s second parameter is "do_disarm_checks"
        if (arming_disarm(&fms.arming, ARMING_CHECK_MAVLINK, !forced)) {
            return MAV_RESULT_ACCEPTED;
        }
        return MAV_RESULT_FAILED;
    }

    return MAV_RESULT_UNSUPPORTED;
}

static MAV_RESULT fms_handle_command_do_set_mode(const mavlink_command_long_t *packet)
{
    const MAV_MODE _base_mode = (MAV_MODE)packet->param1;
    const uint32_t _custom_mode = (uint32_t)packet->param2;

    return fms_set_mode_common(_base_mode, _custom_mode);
}

/*
  code common to both SET_MODE mavlink message and command long set_mode msg
*/
static MAV_RESULT fms_set_mode_common(const MAV_MODE _base_mode, const uint32_t _custom_mode)
{
    // only accept custom modes because there is no easy mapping from Mavlink flight modes to AC flight modes
    if ((uint32_t)_base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
        if (!fms_set_mode(_custom_mode, MODE_REASON_GCS_COMMAND)) {
            // often we should be returning DENIED rather than FAILED
            // here.  Perhaps a "has_mode" callback on AP_::vehicle()
            // would do?
            return MAV_RESULT_FAILED;
        }
        return MAV_RESULT_ACCEPTED;
    }

    if (_base_mode == (MAV_MODE)MAV_MODE_FLAG_DECODE_POSITION_SAFETY) {
        // set the safety switch position. Must be in a command by itself
        if (_custom_mode == 0) {
            // turn safety off (pwm outputs flow to the motors)
            //hal.rcout->force_safety_off();
            return MAV_RESULT_ACCEPTED;
        }
        if (_custom_mode == 1) {
#if 0
            // turn safety on (no pwm outputs to the motors)
            if (hal.rcout->force_safety_on()) {
                return MAV_RESULT_ACCEPTED;
            }
            return MAV_RESULT_FAILED;
#endif
            return MAV_RESULT_ACCEPTED;
        }
        return MAV_RESULT_DENIED;
    }

    // Command is invalid (is supported but has invalid parameters)
    return MAV_RESULT_DENIED;
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void fms_gcs_cb(void *parameter)
{
    uitc_mavlink_msg *mavlink_msg = (uitc_mavlink_msg *)parameter;
    uint32_t msg_id;
    uint8_t *msg_buf;
    uint32_t msg_len;

    switch (mavlink_msg->msg_t->msgid) {
        case MAVLINK_MSG_ID_COMMAND_LONG: {
            mavlink_command_long_t command;
            mavlink_msg_command_long_decode(mavlink_msg->msg_t, &command);

            if (fms_should_handle_command_long(command.command)) {
                fms_push_msg(MAVLINK_MSG_ID_COMMAND_LONG, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&command, sizeof(mavlink_command_long_t));
            }
        } break;

        case MAVLINK_MSG_ID_COMMAND_INT:
        {
            mavlink_command_int_t command;
            mavlink_msg_command_int_decode(mavlink_msg->msg_t, &command);

            if (fms_should_handle_command_int(command.command)) {
                fms_push_msg(MAVLINK_MSG_ID_COMMAND_INT, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&command, sizeof(mavlink_command_int_t));
            }
            break;
        }

        case MAVLINK_MSG_ID_MANUAL_CONTROL:
        {
            mavlink_manual_control_t packet;
            mavlink_msg_manual_control_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_MANUAL_CONTROL, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_manual_control_t));
            break;
        }
    
        case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
        {
            mavlink_rc_channels_override_t packet;
            mavlink_msg_rc_channels_override_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_rc_channels_override_t));
            break;
        }

        case MAVLINK_MSG_ID_SET_MODE:
        {
            mavlink_set_mode_t packet;
            mavlink_msg_set_mode_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_SET_MODE, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_set_mode_t));
            break;
        }

        case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:   // MAV ID: 82
        {
            mavlink_set_attitude_target_t packet;
            mavlink_msg_set_attitude_target_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_set_attitude_target_t));
            break;
        }

        case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:     // MAV ID: 84
        {
            // decode packet
            mavlink_set_position_target_local_ned_t packet;
            mavlink_msg_set_position_target_local_ned_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_set_position_target_local_ned_t));
            break;
        }

        case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:    // MAV ID: 86
        {
            // decode packet
            mavlink_set_position_target_global_int_t packet;
            mavlink_msg_set_position_target_global_int_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_set_position_target_global_int_t));
            break;
        }

        case MAVLINK_MSG_ID_MISSION_ITEM:    // MAV ID: 39
        {
            // decode packet
            mavlink_mission_item_t packet;
            mavlink_msg_mission_item_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_MISSION_ITEM, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_mission_item_t));
            break;
        }

        case MAVLINK_MSG_ID_MISSION_ITEM_INT:    // MAV ID: 73
        {
            // decode packet
            mavlink_mission_item_int_t packet;
            mavlink_msg_mission_item_int_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_MISSION_ITEM_INT, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_mission_item_int_t));
            break;
        }
   
        default: {
            // console_printf("unknown mavlink msg:%d\n", msg->msgid);
        } break;
    }
}

/**
  * @brief       
  * @param[in]   msg_id  
  * @param[in]   msg_buf  
  * @param[in]   msg_len  
  * @param[out]  
  * @retval      
  * @note        
  */
static void fms_push_msg(uint32_t msg_id, uint8_t sysid, uint8_t compid, uint8_t *msg_buf, uint32_t msg_len)
{
    if (rt_ringbuffer_space_len(&gcs_msg.msg_rb) >= msg_len + sizeof(msg_id) + sizeof(sysid) + sizeof(compid)) {

        rt_mutex_take(&gcs_msg._mutex, RT_WAITING_FOREVER);

        rt_ringbuffer_put(&gcs_msg.msg_rb, (uint8_t *)&msg_id, sizeof(msg_id));
        rt_ringbuffer_putchar(&gcs_msg.msg_rb, sysid);
        rt_ringbuffer_putchar(&gcs_msg.msg_rb, compid);
        rt_ringbuffer_put(&gcs_msg.msg_rb, msg_buf, msg_len);

        rt_mutex_release(&gcs_msg._mutex);
    } else {
        mavproxy_send_statustext(MAV_SEVERITY_INFO, "fms_gcs miss msg");
    }
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static uint32_t fms_get_msg_id()
{
    uint32_t msg_id;
    rt_mutex_take(&gcs_msg._mutex, RT_WAITING_FOREVER);
    rt_ringbuffer_get(&gcs_msg.msg_rb, (uint8_t *)&msg_id, sizeof(msg_id));
    rt_mutex_release(&gcs_msg._mutex);

    return msg_id;
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void fms_get_msg_date(uint8_t *msg_buf, uint32_t msg_len)
{
    rt_mutex_take(&gcs_msg._mutex, RT_WAITING_FOREVER);
    rt_ringbuffer_getchar(&gcs_msg.msg_rb, &gcs_msg.sysid);
    rt_ringbuffer_getchar(&gcs_msg.msg_rb, &gcs_msg.compid);
    rt_ringbuffer_get(&gcs_msg.msg_rb, msg_buf, msg_len);
    rt_mutex_release(&gcs_msg._mutex);
}

static bool fms_should_handle_command_long(uint16_t command)
{
    bool res = false;

    switch (command) {
    case MAV_CMD_COMPONENT_ARM_DISARM:
    case MAV_CMD_DO_SET_MODE:
    case MAV_CMD_NAV_TAKEOFF:
    case MAV_CMD_GET_HOME_POSITION:
        res = true;
        break;
    default:
        break;
    }

    return res;
}

static bool fms_should_handle_command_int(uint16_t command)
{
    bool res = false;

    switch (command) {
    case MAV_CMD_DO_REPOSITION:
    case MAV_CMD_DO_PAUSE_CONTINUE:
        res = true;
        break;
    default:
        break;
    }

    return res;
}

static bool mavlink_coordinate_frame_to_location_alt_frame(const MAV_FRAME coordinate_frame, AltFrame *frame)
{
    switch (coordinate_frame) {
    case MAV_FRAME_GLOBAL_RELATIVE_ALT: // solo shot manager incorrectly sends RELATIVE_ALT instead of RELATIVE_ALT_INT
    case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
        *frame = ALT_FRAME_ABOVE_HOME;
        return true;
    case MAV_FRAME_GLOBAL_TERRAIN_ALT:
    case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT:
        *frame = ALT_FRAME_ABOVE_TERRAIN;
        return true;
    case MAV_FRAME_GLOBAL:
    case MAV_FRAME_GLOBAL_INT:
        *frame = ALT_FRAME_ABSOLUTE;
        return true;
    default:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
        mavproxy_send_statustext(MAV_SEVERITY_INFO, "Unknown mavlink coordinate frame %u", coordinate_frame);
#endif
        return false;
    }
}

static MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t *packet)
{
    const bool change_modes = ((int32_t)packet->param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) == MAV_DO_REPOSITION_FLAGS_CHANGE_MODE;
    if (!mode_in_guided_mode(fms.flightmode) && !change_modes) {
        return MAV_RESULT_DENIED;
    }

    // sanity check location
    if (!location_check_lat_lng2(packet->x, packet->y)) {
        return MAV_RESULT_DENIED;
    }

    Location request_location;
    if (!location_from_command_t(packet, &request_location)) {
        return MAV_RESULT_DENIED;
    }

    if (location_sanitize(&request_location, &fms.ahrs->curr_loc)) {
        // if the location wasn't already sane don't load it
        return MAV_RESULT_DENIED; // failed as the location is not valid
    }

    // we need to do this first, as we don't want to change the flight mode unless we can also set the target
    if (!mguided_set_destination2((mode_base_t)&fms.mode_guided, &request_location, false, 0, false, 0, false)) {
        return MAV_RESULT_FAILED;
    }

    if (!mode_in_guided_mode(fms.flightmode)) {
        if (!fms_set_mode(GUIDED, MODE_REASON_GCS_COMMAND)) {
            return MAV_RESULT_FAILED;
        }
        // the position won't have been loaded if we had to change the flight mode, so load it again
        if (!mguided_set_destination2((mode_base_t)&fms.mode_guided, &request_location, false, 0, false, 0, false)) {
            return MAV_RESULT_FAILED;
        }
    }

    return MAV_RESULT_ACCEPTED;
}

static bool location_from_command_t(const mavlink_command_int_t *in, Location *out)
{
    if (!command_long_stores_location((MAV_CMD)in->command)) {
        return false;
    }

    // integer storage imposes limits on the altitudes we can accept:
    if (fabsf(in->z) > LOCATION_ALT_MAX_M) {
        return false;
    }

    AltFrame frame;
    if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)in->frame, &frame)) {
        // unknown coordinate frame
        return false;
    }

    out->lat = in->x;
    out->lng = in->y;

    location_set_alt_cm(out, (int32_t)(in->z * 100), frame);

    return true;
}

static bool command_long_stores_location(const MAV_CMD command)
{
    switch(command) {
    case MAV_CMD_DO_SET_HOME:
    case MAV_CMD_DO_SET_ROI:
    case MAV_CMD_DO_SET_ROI_LOCATION:
    case MAV_CMD_NAV_TAKEOFF:
    case MAV_CMD_DO_REPOSITION:
        return true;
    default:
        return false;
    }
    return false;
}

static MAV_RESULT handle_command_pause_continue(const mavlink_command_int_t *packet)
{
    // requested pause
    if ((uint8_t) packet->param1 == 0) {
        if (mode_pause((mode_base_t)fms.flightmode)) {
            return MAV_RESULT_ACCEPTED;
        }
        mavproxy_send_statustext(MAV_SEVERITY_INFO, "Failed to pause");
        return MAV_RESULT_FAILED;
    }

    // requested resume
    if ((uint8_t) packet->param1 == 1) {
        if (mode_resume((mode_base_t)fms.flightmode)) {
            return MAV_RESULT_ACCEPTED;
        }
        mavproxy_send_statustext(MAV_SEVERITY_INFO, "Failed to resume");
        return MAV_RESULT_FAILED;
    }
    return MAV_RESULT_DENIED;
}


/// mission
/*
  handle an incoming mission item
  return true if this is the last mission item, otherwise false
 */
static bool handle_mission_item(mavlink_message_t *msg)
{
    MAV_MISSION_RESULT result = MAV_MISSION_ACCEPTED;
    bool mission_is_complete = false;
    // TODO: rename packet to mission_item_int
    mavlink_mission_item_int_t packet = {0};
    Mission_Command cmd = {0};
    uint16_t seq = 0;
    uint8_t current = 0;
    
    if (msg->msgid == MAVLINK_MSG_ID_MISSION_ITEM) {
        mavlink_mission_item_t mission_item;
        mavlink_msg_mission_item_decode(msg, &mission_item);
        MAV_MISSION_RESULT ret = mission_convert_MISSION_ITEM_to_MISSION_ITEM_INT(&mission_item, &packet);
        ret = mission_MavlinkIntToMissionCmd(&packet, &cmd);
        if (ret != MAV_MISSION_ACCEPTED) {
            goto mission_ack;
        }
    } else {
        mavlink_msg_mission_item_int_decode(msg, &packet);
        MAV_MISSION_RESULT ret = mission_MavlinkIntToMissionCmd(&packet, &cmd);
        if (ret != MAV_MISSION_ACCEPTED) {
            goto mission_ack;
        }
    }
    current = packet.current;
    seq = packet.seq;
    
    if ((current == 2 || current == 3)) {
        MAV_MISSION_RESULT result = mission_MavlinkIntToMissionCmd(&packet, &cmd);
        if (result != MAV_MISSION_ACCEPTED) {
            //decode failed
            goto mission_ack;
        }
        // guided or change-alt
        if (current == 2) {
            // current = 2 is a flag to tell us this is a "guided mode"
            // waypoint and not for the mission
            result = (handle_guided_request(&cmd) ? MAV_MISSION_ACCEPTED
                      : MAV_MISSION_ERROR) ;
        } else if (current == 3) {
            //current = 3 is a flag to tell us this is a alt change only
            // add home alt if needed

            // verify we recevied the command
            result = MAV_MISSION_ACCEPTED;
        }
        goto mission_ack;
    }
    
    return mission_is_complete;
mission_ack:
    // we are rejecting the mission/waypoint
    send_mission_ack(msg, result);
    
    return mission_is_complete;
}

/**
  * @brief       任务回复
  * @param[in]   msg  
  * @param[out]  
  * @retval      
  * @note        mavlink发送或打包函数中的sysid和compid是飞控的，
  * 结构体中的，是地面站的
  */
static void send_mission_ack(mavlink_message_t *msg,
                      MAV_MISSION_RESULT result)
{
    mavlink_system_t sysid_this_mav = mavproxy_get_system();
    mavlink_mission_ack_t mission_ack = {msg->sysid,
                                         msg->compid,
                                         result,
                                         MAV_MISSION_TYPE_MISSION};

    mavlink_msg_mission_ack_encode(sysid_this_mav.sysid, sysid_this_mav.compid, msg, &mission_ack);
    mavproxy_send_immediate_msg(msg, 1);
}

static bool handle_guided_request(Mission_Command *cmd)
{
    // only process guided waypoint if we are in guided mode
    if (mode_number(fms.flightmode) != GUIDED) {
       return false;
    }

    // switch to handle different commands
    switch (cmd->id) {

        case MAV_CMD_NAV_WAYPOINT:
        {
          // set wp_nav's destination
          Location dest = cmd->content.location;
          return mguided_set_destination2((mode_base_t)&fms.mode_guided, &dest, false, 0.0f, false, 0.0f, false);
        }

        case MAV_CMD_CONDITION_YAW:
          do_yaw(cmd);
          return true;

        default:
          // reject unrecognised command
          return false;
    }

    return true;
}

static void do_yaw(const Mission_Command* cmd)
{
    autoyaw_set_fixed_yaw(fms.flightmode->auto_yaw,
        cmd->content.yaw.angle_deg,
        cmd->content.yaw.turn_rate_dps,
        cmd->content.yaw.direction,
        cmd->content.yaw.relative_angle > 0);
}

static MAV_RESULT handle_command_get_home_position(const mavlink_command_long_t *packet, mavlink_message_t* msg)
{
    if (!ahrs_home_is_set(fms.ahrs)) {
        return MAV_RESULT_FAILED;
    }

    send_home_position(msg);
    send_gps_global_origin(msg);

    return MAV_RESULT_ACCEPTED;
}

static void send_home_position(mavlink_message_t* msg)
{
    if (!ahrs_home_is_set(fms.ahrs)) {
        return;
    }

    Location home = {0};
    ahrs_get_home(fms.ahrs, &home);

    mavlink_system_t sysid_this_mav = mavproxy_get_system();

    mavlink_home_position_t home_position = {.latitude = home.lat,
                                             .longitude = home.lng,
                                             .altitude = home.alt * 10,
                                             .x = 0.0f, .y = 0.0f, .z = 0.0f,
                                             .q[0] = 1.0f, .q[1] = 0.0f,.q[2] = 0.0f,.q[3] = 0.0f,
                                             .approach_x = 0.0f, .approach_y = 0.0f, .approach_z = 0.0f,
                                             .time_usec = time_micros64()};

    mavlink_msg_home_position_encode(sysid_this_mav.sysid, sysid_this_mav.compid, msg, &home_position);
}

static void send_gps_global_origin(mavlink_message_t* msg)
{
    Location ekf_origin = {0};
    if (!ahrs_get_origin(fms.ahrs, &ekf_origin)) {
        return;
    }

    mavlink_system_t sysid_this_mav = mavproxy_get_system();

    mavlink_gps_global_origin_t global_origin = {.latitude = ekf_origin.lat,
                                                 .longitude = ekf_origin.lng,
                                                 .altitude = ekf_origin.alt * 10,
                                                 .time_usec = time_micros64()};

    mavlink_msg_gps_global_origin_encode(sysid_this_mav.sysid, sysid_this_mav.compid, msg, &global_origin);
}

/*------------------------------------test------------------------------------*/


